/*
@hehung
email: 1398660197@qq.com
wechat: hehung95
reproduced and please indicate the source @hehung
*/

/*
	INT - P804
*/
#include "app_common.h"
#include <math.h>
#include "app_max30102_algorithm.h"

#define MAX30102_DEBUG
#undef  MAX30102_DEBUG

#ifdef MAX30102_DEBUG
#include <stdio.h>
#define LOG(fmt, ...) 	          printf(fmt, ##__VA_ARGS__)
#else
#define LOG(fmt, ...)
#endif


#define  MAX30102_DETECT_THRESHOLD           (20000)


static uint32_t fifo_red;
static uint32_t fifo_ir;

static uint16_t g_fft_index = 0;         	 	
static compx s1[MAX30102_FFT_NUM+16];           	
static compx s2[MAX30102_FFT_NUM+16];           	

static BloodData g_blooddata = {0, 0.00};					


static bool max30102_working_flag = false;

static void Max30102_WriteToI2C(uint8_t Register_Address, uint8_t Data);
static uint8_t Max30102_ReadFromI2C(uint8_t Register_Address);
static uint8_t Max30102_ReadI2CMulti(uint8_t register_addr, uint8_t *Data, uint16_t Num);
static void Max30102_Setting(void);
static void Max30102_ReadFifo(void);

// Print the data to upper computer ANO_TC
void Anotc_WavefromUserDataPrintf(uint8_t user_data_ID, uint32_t send_data)
{
	uint8_t send_ch[8];
	uint8_t checksum;
	uint32_t sum = 0;
	uint8_t i;

	send_ch[0] = 0xAA;
	send_ch[1] = 0xAA;

	send_ch[2] = 0xF1 + user_data_ID;

	send_ch[3] = 0x04;
	send_ch[4] = (send_data >> 24) & 0xFF;
	send_ch[5] = (send_data >> 16) & 0xFF;
	send_ch[6] = (send_data >> 8) & 0xFF;
	send_ch[7] = (send_data) & 0xFF;

	for (i = 0; i < 8; i++)
	{
		sum += send_ch[i];
		printf ("%c", send_ch[i]);
	}

	checksum = sum & 0xFF;
	printf ("%c", checksum);
}

// external interrupt callback function for max30102
void irq01_callback(external_irq_callback_args_t *p_args)
{
	BaseType_t  event_status;
	EventBits_t setting_event_group;
	BaseType_t xHigherPriorityTaskWoken = pdFALSE;
	
	if (1 == p_args->channel)
	{
		setting_event_group = EVENT_GROUP_SET_BIT(EVENT_GROUP_1_MAX30102);
		event_status = xEventGroupSetBitsFromISR(event_group_0, setting_event_group, &xHigherPriorityTaskWoken);
		if (event_status == pdFALSE)
		{
			LOG ("Max30102: event group 0 setting error - init\n");
		}
	}
}

// time1 2ms callback function for max30102
void timer1_2ms_callback(timer_callback_args_t *p_args)
{
    FSP_PARAMETER_NOT_USED(p_args);

//	ppg_time_ms += 2;
}


/***********************************************************************************************
    I2C for max30102 transmission
************************************************************************************************/
// Write one byte to I2c
static void Max30102_WriteToI2C(uint8_t Register_Address, uint8_t Data)
{
	fsp_err_t err;
	uint32_t timeout_ms = 500;
	uint8_t transmit_data[2] = {Register_Address, Data};

	// setting mutex for lock sci3_i2c bus
    if (pdTRUE == xSemaphoreTake(mutex_sci3_i2c, portMAX_DELAY))
    {
		R_SCI_I2C_SlaveAddressSet(&g_sci3_i2c_ctrl, 
									MAX30102_I2C_SLAVE_ADDR_7BIT, 
									I2C_MASTER_ADDR_MODE_7BIT);

		err = R_SCI_I2C_Write(&g_sci3_i2c_ctrl, transmit_data, 2, true);
		assert(FSP_SUCCESS == err);
		
		/* Since there is nothing else to do, block until Callback triggers*/
		while ((I2C_MASTER_EVENT_TX_COMPLETE != sci3_i2c_event) && timeout_ms>0)
		{
			R_BSP_SoftwareDelay(1U, BSP_DELAY_UNITS_MICROSECONDS);
			timeout_ms--;
		}
		
		if (I2C_MASTER_EVENT_ABORTED == sci3_i2c_event)
		{
			// __BKPT(0);
			LOG ("Max30102: sci3 i2c write event aborted occurred\r\n");
		}
		/* Read data back from the I2C slave */
		sci3_i2c_event = I2C_MASTER_EVENT_ABORTED;

 		// release the mutex, unlock
        (void)xSemaphoreGive(mutex_sci3_i2c);
    }
}

// Read one byte from EEPROM
static uint8_t Max30102_ReadFromI2C(uint8_t Register_Address)
{
	fsp_err_t err;
	uint32_t timeout_ms = 500;
	uint8_t transmit_data = 0;

    // setting mutex for lock sci3_i2c bus
    if (pdTRUE == xSemaphoreTake(mutex_sci3_i2c, portMAX_DELAY))
    {
		R_SCI_I2C_SlaveAddressSet(&g_sci3_i2c_ctrl, 
								MAX30102_I2C_SLAVE_ADDR_7BIT, 
								I2C_MASTER_ADDR_MODE_7BIT);
		/* Write address */
		err = R_SCI_I2C_Write(&g_sci3_i2c_ctrl, &Register_Address, 1, true);
		assert(FSP_SUCCESS == err);
		
		/* Since there is nothing else to do, block until Callback triggers*/
		while ((I2C_MASTER_EVENT_TX_COMPLETE != sci3_i2c_event) && timeout_ms>0)
		{
			R_BSP_SoftwareDelay(1U, BSP_DELAY_UNITS_MICROSECONDS);
			timeout_ms--;
		}
		
		if (I2C_MASTER_EVENT_ABORTED == sci3_i2c_event)
		{
			// __BKPT(0);
			LOG ("Max30102: sci3 i2c write event aborted occurred\r\n");
		}
		/* Read data back from the I2C slave */
		sci3_i2c_event = I2C_MASTER_EVENT_ABORTED;

		/* Read data from max30102 */
		err = R_SCI_I2C_Read(&g_sci3_i2c_ctrl, &transmit_data, 1, true);
		assert(FSP_SUCCESS == err);
		
		/* Since there is nothing else to do, block until Callback triggers*/
		while ((I2C_MASTER_EVENT_TX_COMPLETE != sci3_i2c_event) && timeout_ms>0)
		{
			R_BSP_SoftwareDelay(1U, BSP_DELAY_UNITS_MICROSECONDS);
			timeout_ms--;
		}
		
		if (I2C_MASTER_EVENT_ABORTED == sci3_i2c_event)
		{
			// __BKPT(0);
			LOG ("Max30102: sci3 i2c read event aborted occurred\r\n");
		}
		/* Read data back from the I2C slave */
		sci3_i2c_event = I2C_MASTER_EVENT_ABORTED;

 		// release the mutex, unlock
        (void)xSemaphoreGive(mutex_sci3_i2c);
    }
	
	return transmit_data;
}

static uint8_t Max30102_ReadI2CMulti(uint8_t Register_Address, uint8_t *Data, uint16_t Num)
{
	fsp_err_t err;
	uint32_t timeout_ms = 500;
	// uint8_t transmit_data = 0;

    // setting mutex for lock sci3_i2c bus
    if (pdTRUE == xSemaphoreTake(mutex_sci3_i2c, portMAX_DELAY))
    {
		R_SCI_I2C_SlaveAddressSet(&g_sci3_i2c_ctrl, 
								MAX30102_I2C_SLAVE_ADDR_7BIT, 
								I2C_MASTER_ADDR_MODE_7BIT);
		/* Write address */
		err = R_SCI_I2C_Write(&g_sci3_i2c_ctrl, &Register_Address, 1, true);
		assert(FSP_SUCCESS == err);
		
		/* Since there is nothing else to do, block until Callback triggers*/
		while ((I2C_MASTER_EVENT_TX_COMPLETE != sci3_i2c_event) && timeout_ms>0)
		{
			R_BSP_SoftwareDelay(1U, BSP_DELAY_UNITS_MICROSECONDS);
			timeout_ms--;
		}
		
		if (I2C_MASTER_EVENT_ABORTED == sci3_i2c_event)
		{
			// __BKPT(0);
			LOG ("Max30102: sci3 i2c write event aborted occurred\r\n");
		}
		/* Read data back from the I2C slave */
		sci3_i2c_event = I2C_MASTER_EVENT_ABORTED;

		/* Read data from max30102 */
		err = R_SCI_I2C_Read(&g_sci3_i2c_ctrl, Data, Num, true);
		assert(FSP_SUCCESS == err);
		
		/* Since there is nothing else to do, block until Callback triggers*/
		while ((I2C_MASTER_EVENT_TX_COMPLETE != sci3_i2c_event) && timeout_ms>0)
		{
			R_BSP_SoftwareDelay(1U, BSP_DELAY_UNITS_MICROSECONDS);
			timeout_ms--;
		}
		
		if (I2C_MASTER_EVENT_ABORTED == sci3_i2c_event)
		{
			// __BKPT(0);
			LOG ("Max30102: sci3 i2c read event aborted occurred\r\n");
		}
		/* Read data back from the I2C slave */
		sci3_i2c_event = I2C_MASTER_EVENT_ABORTED;

 		// release the mutex, unlock
        (void)xSemaphoreGive(mutex_sci3_i2c);
    }
	
	return 1;
}

/***********************************************************************************************
    Data handler
************************************************************************************************/
// Configuration the Max30102
static void Max30102_Setting(void)
{
	// Reset max30102
	Max30102_WriteToI2C(REG_MODE_CONFIG,0x40);
	
	DELAY_MS(100);
	
	// interrupt enable: FIFO almost full flag, new FIFO Data Ready
	Max30102_WriteToI2C(REG_INTR_ENABLE_1, 0xC0);	// INTR setting
	Max30102_WriteToI2C(REG_INTR_ENABLE_2, 0x00);

	Max30102_WriteToI2C(REG_FIFO_WR_PTR, 0x00);  	// FIFO_WR_PTR[4:0]
	Max30102_WriteToI2C(REG_OVF_COUNTER, 0x00);  	// OVF_COUNTER[4:0]
	Max30102_WriteToI2C(REG_FIFO_RD_PTR, 0x00);  	// FIFO_RD_PTR[4:0]

	Max30102_WriteToI2C(REG_FIFO_CONFIG, 0x0f);  	// sample avg = 1, fifo rollover=false, fifo almost full = 17
	Max30102_WriteToI2C(REG_MODE_CONFIG, 0x03);  	// 0x02 for Red only, 0x03 for SpO2 mode 0x07 multimode LED

	Max30102_WriteToI2C(REG_SPO2_CONFIG, 0x27);  	// SPO2_ADC range = 4096nA, SPO2 sample rate (100 Hz), LED pulseWidth (400uS) 
	Max30102_WriteToI2C(REG_LED1_PA, 0x24);   	// Choose value for ~ 7mA for LED1
	Max30102_WriteToI2C(REG_LED2_PA, 0x24);   	// Choose value for ~ 7mA for LED2
	Max30102_WriteToI2C(REG_PILOT_PA, 0x7f);   	// Choose value for ~ 25mA for Pilot LED
}

/***********************************************************************************************
    Max30102 data read
************************************************************************************************/
// Read data from Max30102
static void Max30102_ReadFifo(void)
{
	uint8_t read_data[6];
	
	fifo_red = 0.0f;
	fifo_ir = 0.0f;

	// read and clear status register
	(void)Max30102_ReadFromI2C(REG_INTR_STATUS_1);
	(void)Max30102_ReadFromI2C(REG_INTR_STATUS_2);

	(void)Max30102_ReadI2CMulti(REG_FIFO_DATA, 
								read_data, 
								6);

	fifo_red = ((read_data[0] << 16) | (read_data[1] << 8) | (read_data[2])) & 0x03ffff;
	fifo_ir = ((read_data[3] << 16) | (read_data[4] << 8) | (read_data[5])) & 0x03ffff;
	
}
 

// Read data from Max30102 sampled
static void Max30102_BloodDataUpdate(void)
{
	
	EventBits_t event_group_bits;
	EventBits_t waiting_eg_bits;
	
	g_fft_index = 0;
	while(g_fft_index < MAX30102_FFT_NUM)
	{
		// waiting for data sample completed
		waiting_eg_bits = EVENT_GROUP_SET_BIT(EVENT_GROUP_1_MAX30102);
		event_group_bits = xEventGroupWaitBits(event_group_0,     
												waiting_eg_bits,            
												pdFALSE,             // Do not clear event bits    
												pdTRUE,              // waiting all bits setting
												portMAX_DELAY);      // waiting forever 
		xEventGroupClearBits(event_group_0, EVENT_GROUP_1_MAX30102);
	
		//read from MAX30102 FIFO2
		Max30102_ReadFifo();  

//		if(g_fft_index < MAX30102_FFT_NUM)
		{
			s1[g_fft_index].real = (float)fifo_red;
			s1[g_fft_index].imag= 0;
			s2[g_fft_index].real = (float)fifo_ir;
			s2[g_fft_index].imag= 0;
			g_fft_index++;
		}
	}	
}

static void Max30102_HearRateProcessing(void)
{
	
}

// Processing the data
static void Max30102_BloodDataProcessing(void)
{
	float n_denom;
	uint16_t i;
	float dc_red = 0; 
	float dc_ir = 0;
	float ac_red = 0; 
	float ac_ir = 0;
	
	// Filter 
	for (i = 0; i < MAX30102_FFT_NUM; i++) 
	{
		dc_red += s1[i].real;
		dc_ir  += s2[i].real;
	}

	dc_red = dc_red / MAX30102_FFT_NUM;
	dc_ir = dc_ir / MAX30102_FFT_NUM;
	
	LOG ("******************************\r\n%f--%f\r\n", dc_red, dc_ir);
	
	if ((dc_red <= MAX30102_DETECT_THRESHOLD) || (dc_ir < MAX30102_DETECT_THRESHOLD))
	{
		max30102_working_flag = false;
	}
	else
	{
		max30102_working_flag = true;
	}

	if (true == max30102_working_flag)
	{
		// Filter
		for (i = 0; i < MAX30102_FFT_NUM; i++)  
		{
			s1[i].real = s1[i].real - dc_red ; 
			s2[i].real = s2[i].real - dc_ir ; 
		}
		
		LOG("*********** 8 pt Moving Average red/ir *************\r\n");

		for(i = 1; i < MAX30102_FFT_NUM - 1; i++) 
		{
			n_denom= (s1[i-1].real + 2*s1[i].real + s1[i+1].real);
			s1[i].real = (float)(n_denom / 4.00); 

			n_denom= (s2[i-1].real + 2*s2[i].real + s2[i+1].real);
			s2[i].real = (uint8_t)(n_denom / 4.00); 			
		}

		for(i = 0; i < MAX30102_FFT_NUM - 8; i++) 
		{
			n_denom = (s1[i].real + s1[i+1].real + s1[i+2].real + s1[i+3].real + s1[i+4].real + 
					s1[i+5].real + s1[i+6].real + s1[i+7].real);
			s1[i].real = (uint8_t)(n_denom / 8.00); 
			
			n_denom = (s2[i].real + s2[i+1].real + s2[i+2].real + s2[i+3].real + s2[i+4].real + 
					s2[i+5].real + s2[i+6].real + s2[i+7].real);
			s2[i].real =  (uint8_t)(n_denom / 8.00); 	
		}

#ifdef MAX30102_DEBUG
		for(i = 0; i < MAX30102_FFT_NUM; i++) 
		{
			LOG("%f, %f\r\n", s1[i].real, s2[i].real);	
		}
#endif /* MAX30102_DEBUG */
		
		g_fft_index = 0;	
		FFT(s1);
		FFT(s2);
		LOG("************** Start FFT (red/ir) ************\r\n");
		for(i = 0; i < MAX30102_FFT_NUM; i++) 
		{
			s1[i].real = sqrtf(s1[i].real * s1[i].real + s1[i].imag * s1[i].imag);
			s1[i].real = sqrtf(s2[i].real * s2[i].real + s2[i].imag * s2[i].imag);
			
			if (i != 0)
			{
				ac_red += s1[i].real;
				ac_ir  += s2[i].real;				
			}
		}
		
#ifdef MAX30102_DEBUG
		for(i = 0; i < MAX30102_FFT_NUM / 2; i++) 
		{
			LOG("%f, %f\r\n",s1[i].real, s2[i].real);
		}
#endif /* MAX30102_DEBUG */
		
		LOG("************** End FFT ************\r\n");
		
		//UsartPrintf(USART_DEBUG,"%d\r\n",(int)i);

		int s1_max_index = find_max_num_index(s1, 30);
//		int s2_max_index = find_max_num_index(s2, 30);
		LOG("%d\r\n", s1_max_index);
//		LOG("%d\r\n", s2_max_index);
		float Heart_Rate = (float)(60.00 * ((100.0 * s1_max_index ) / 512.00) + 20.0);
		
		g_blooddata.heart = (int)Heart_Rate;
		
		float R = (ac_ir*dc_red) / (ac_red*dc_ir);
		float sp02_num = (float)(-45.060 * R * R + 30.354 *R + 94.845);
		g_blooddata.SpO2 = sp02_num;
		
		g_blooddata.SpO2 = (g_blooddata.SpO2 > 99.99) ? (float)99.99 : g_blooddata.SpO2;
	}	
}

// Max30102 data processing
void Max30102_Processing(void)
{
	Max30102_BloodDataUpdate();
	Max30102_BloodDataProcessing();

	LOG ("BR: %3d\r\n" , g_blooddata.heart);
	LOG ("SO2: %0.2f\r\n" , g_blooddata.SpO2);
}

/***********************************************************************************************
    Max30102 initialization
************************************************************************************************/
// Initialization for MAX30102
void Max30102_Init(void)
{
	fsp_err_t err = FSP_SUCCESS;
	
	// Initialize the timer for heart rate counter
    g_timer1.p_api->open(g_timer1.p_ctrl, g_timer1.p_cfg);
    g_timer1.p_api->start(g_timer1.p_ctrl);
	
	// Open interrupt pin as the external interrupt
	err = R_ICU_ExternalIrqOpen(&g_external_irq1_ctrl, &g_external_irq1_cfg);
	(void)err;
	// Enable external interrupt pin
	err = R_ICU_ExternalIrqEnable(&g_external_irq1_ctrl);
	(void)err;
	
	// Max30102 setting
	Max30102_Setting();
}


bool Max30102_GetValidFlag(void)
{
	return max30102_working_flag;
}

BloodData Max30102_GetSampleValue(void)
{
	return g_blooddata;
}

